//
//  main.cpp
//  DCA
//
//  Created by James Critchley on 7/26/11.
//  Copyright 2011 __MyCompanyName__. All rights reserved.
//

#include <iostream>
#include <pthread.h>

#include "Sys3D.h"
#include "Quaternion.h"
#include "Rotation.h"

#include "DCFlowControl.h"
#include "DCAComputation.h"

#include "Timer.h"

void *ThreadFunction(void *s)
{        
    DCFlowControl *dca = (DCFlowControl*) s;
    while(1){
            dca->assembly_traversal();                
            dca->solve_traversal();
    }
    return 0;
}

int main (int argc, const char * argv[])
{
        const double& q = 1.0;
        const double& u = 1.0;
        
        const int& nbodies = 512;
        const int& nproc = 4;
        const int& niter = 1000;
        int x,y,z;
        int start_id, end_id, start_asm;
        x = 25;
        y = nbodies - 3*x;
        z = nbodies/4;
        
        RSIM::Sys3D mbs3D(nbodies,nproc);
        mbs3D.initDCANodesForDCA();        
        mbs3D.setQ(q);
        mbs3D.setU(u);
             
        // declair the Divide & Conquer (DC) structure
        DCBinaryParallelFlow dca0;                
        
        DCBinaryParallelFlow dca11;
        DCBinaryParallelFlow dca12;
        
        DCFlowTerminator dca21;
        DCFlowTerminator dca22;
        DCFlowTerminator dca23;
        DCFlowTerminator dca24;
        
        // establish the flow through "child" linkages
        dca0.child1 = &dca11;
        dca0.child2 = &dca12;
        
        dca11.child1 = &dca21;
        dca11.child2 = &dca22;
        
        dca12.child1 = &dca23;
        dca12.child2 = &dca24;
        
        // synchronization objects    
        // assign non-null synchronizations to the divisions which represent new threads    
        CommSync sync_0_12;
        CommSync sync_11_22;
        CommSync sync_12_24;
        
        dca0.child_sync = dca12.parent_sync = &sync_0_12;
        dca11.child_sync = dca22.parent_sync = &sync_11_22;
        dca12.child_sync = dca24.parent_sync = &sync_12_24;

        // declair the calculations
        DCARoot comp0;
        DCABasic comp11;
        DCABasic comp12;
        DCA comp21;
        DCA comp22;
        DCA comp23;
        DCA comp24;
        
        // link the computations
        dca0.computation = &comp0;
        dca11.computation = &comp11;
        dca12.computation = &comp12;
        dca21.computation = &comp21;
        dca22.computation = &comp22;
        dca23.computation = &comp23;
        dca24.computation = &comp24;
        
        // associate local data with the computations
        comp0.mbs3D = &mbs3D;
        comp0.parent_id = 3*x+y-4;
        comp0.child_id = 3*x+y-3;
        comp0.asm_id = 3*x+y-2;  
        
        comp11.mbs3D = &mbs3D;
        comp11.parent_id = x-2;
        comp11.child_id = 2*(x-1)-1;
        comp11.asm_id = 3*x+y-4;  
        comp11.calcH2 = true;

        comp12.mbs3D = &mbs3D;
        comp12.parent_id = 3*(x-1)-1;
        comp12.child_id = 3*x+y-5;
        comp12.asm_id = 3*x+y-3;          

        int id_count = 0;
        comp21.mbs3D = &mbs3D;
        comp21.start_id = 0;    comp21.end_id = x-1;      comp21.start_asm = 0;
        comp21.dis_start = x-2; comp21.dis_end = 0;
        comp21.kin_startid = 0; comp21.kin_endid = x-1;
        comp21.calcH2 = true;
        mbs3D.ids_[id_count++] = comp21.kin_startid;
                
        comp22.mbs3D = &mbs3D;
        comp22.start_id = x;    comp22.end_id = 2*x-1;     comp22.start_asm = x-1;
        comp22.dis_start = 2*(x-1)-1; comp22.dis_end = x-1;
        comp22.kin_startid = x; comp22.kin_endid = 2*x-1;       
        comp22.calcH2 = true;
        mbs3D.ids_[id_count++] = comp22.kin_startid;                        

        comp23.mbs3D = &mbs3D;
        comp23.start_id = 2*x;   comp23.end_id = 3*x-1;     comp23.start_asm = 2*(x-1);
        comp23.dis_start = 3*(x-1)-1;   comp23.dis_end = 2*(x-1);
        comp23.kin_startid = 2*x; comp23.kin_endid = 3*x-1;
        comp23.calcH2 = true;
        mbs3D.ids_[id_count++] = comp23.kin_startid;                
        
        comp24.mbs3D = &mbs3D;
        comp24.start_id = 3*x;   comp24.end_id = 3*x+y-1;     comp24.start_asm = 3*(x-1);
        comp24.dis_start = 3*x+y-5; comp24.dis_end = 3*(x-1);
        comp24.kin_startid = 3*x; comp24.kin_endid = nbodies-1;        
        mbs3D.ids_[id_count++] = comp24.kin_startid;
        mbs3D.ids_[id_count++] = comp24.kin_endid+1;
                
        comp0.calcH2 = comp12.calcH2 = comp24.calcH2 = false;
                        
        // launch all non-root threads (there will be 3 in this example)
        pthread_attr_t attr1,attr2,attr3;
        pthread_t thread1,thread2,thread3;

        pthread_attr_init(&attr1);
        pthread_attr_init(&attr2);
        pthread_attr_init(&attr3);
        
        //now override the default attribute with changes to detachstate,
        // schedpolicy, schedparam, inheritsched, and scope
        pthread_attr_setscope(&attr1, PTHREAD_SCOPE_SYSTEM);
        pthread_attr_setscope(&attr2, PTHREAD_SCOPE_SYSTEM);
        pthread_attr_setscope(&attr3, PTHREAD_SCOPE_SYSTEM);
        
        //set detached so thread resources will be freed immediately upon
        // thread termination (no join needed)
        pthread_attr_setdetachstate(&attr1, PTHREAD_CREATE_DETACHED);
        pthread_attr_setdetachstate(&attr2, PTHREAD_CREATE_DETACHED);
        pthread_attr_setdetachstate(&attr3, PTHREAD_CREATE_DETACHED);

        // launch the threads
        void *data = (void *) &dca12;
        pthread_create(&thread1, &attr1, ThreadFunction, data);
        data = (void *) &dca22;
        pthread_create(&thread2, &attr2, ThreadFunction, data);
        data = (void *) &dca24;
        pthread_create(&thread3, &attr3, ThreadFunction, data);

        //clean-up the attributes
        pthread_attr_destroy(&attr1);
        pthread_attr_destroy(&attr2);
        pthread_attr_destroy(&attr3);

        // start a timer
        
        // loop for ???
        
        mbs3D.updRotationMatrix();
        mbs3D.updAngularVelocity();
        
        RSIM::Timer t;
        for(int i=0;i<niter;i++)
        {        
                // run the root subsystem
                dca0.assembly_traversal();        
                dca0.solve_traversal();                

        }    
        t.stop();
        t.printElapsedTime(0);                
//         cout<<setprecision(16);
//         mbs3D.displayUDot();

        exit(0);
        return 0;
}
